#include "ros_json_converter.h"

/*std_msgs/Int8*/
Json::Value std_msgsToJson(const std_msgs::msg::Int8 &msg)
{
    Json::Value json_msg;
    json_msg["data"] = msg.data;

    return json_msg;
}

/*std_msgs/Time*/
// Json::Value std_msgsToJson(const std_msgs::msg::Time &msg)
// {
//     Json::Value json_msg;
//     json_msg["sec"] = msg.data.sec;
//     json_msg["nsec"] = msg.data.nsec;

//     return json_msg;
// }

/*std_msgs/ColorRGBA*/
Json::Value std_msgsToJson(const std_msgs::msg::ColorRGBA &msg)
{
    Json::Value json_msg;
    json_msg["r"] = msg.r;
    json_msg["g"] = msg.g;
    json_msg["b"] = msg.b;
    json_msg["a"] = msg.a;

    return json_msg;
}

/*std/Header*/
Json::Value std_msgsToJson(const std_msgs::msg::Header &msg)
{
    Json::Value json_stamp;
    json_stamp["secs"] = msg.stamp.sec;
    json_stamp["nsecs"] = msg.stamp.nanosec;

    Json::Value json_msg;
    // json_msg["seq"] = msg.seq;
    json_msg["stamp"] = json_stamp;
    json_msg["frame_id"] = msg.frame_id;

    return json_msg;
}

/*Accel*/
Json::Value geometry_msgsToJson(const geometry_msgs::msg::Accel &msg)
{
    Json::Value json_msg;
    json_msg["linear"] = geometry_msgsToJson(msg.linear);
    json_msg["angular"] = geometry_msgsToJson(msg.angular);

    return json_msg;
}
Json::Value geometry_msgsToJson(const geometry_msgs::msg::AccelStamped &msg)
{
    Json::Value json_msg;
    json_msg["header"] = std_msgsToJson(msg.header);
    json_msg["accel"] = geometry_msgsToJson(msg.accel);

    return json_msg;
}
Json::Value geometry_msgsToJson(const geometry_msgs::msg::AccelWithCovariance &msg)
{
    Json::Value covArray(Json::arrayValue);
    for (auto &cov : msg.covariance)
    {
        covArray.append(cov);
    }

    Json::Value json_msg;
    json_msg["accel"] = geometry_msgsToJson(msg.accel);
    json_msg["covariance"] = covArray;

    return json_msg;
}
Json::Value geometry_msgsToJson(const geometry_msgs::msg::AccelWithCovarianceStamped &msg)
{
    Json::Value json_msg;
    json_msg["header"] = std_msgsToJson(msg.header);
    json_msg["accel"] = geometry_msgsToJson(msg.accel);

    return json_msg;
}

/*Inertia*/
Json::Value geometry_msgsToJson(const geometry_msgs::msg::Inertia &msg)
{
    Json::Value json_msg;
    json_msg["m"] = msg.m;
    json_msg["com"] = geometry_msgsToJson(msg.com);
    json_msg["ixx"] = msg.ixx;
    json_msg["ixy"] = msg.ixy;
    json_msg["ixz"] = msg.ixz;
    json_msg["iyy"] = msg.iyy;
    json_msg["iyz"] = msg.iyz;
    json_msg["izz"] = msg.izz;

    return json_msg;
}
Json::Value geometry_msgsToJson(const geometry_msgs::msg::InertiaStamped &msg)
{
    Json::Value json_msg;
    json_msg["header"] = std_msgsToJson(msg.header);
    json_msg["inertia"] = geometry_msgsToJson(msg.inertia);

    return json_msg;
}

/*Point*/
Json::Value geometry_msgsToJson(const geometry_msgs::msg::Point &msg)
{
    Json::Value json_msg;
    json_msg["x"] = msg.x;
    json_msg["y"] = msg.y;
    json_msg["z"] = msg.z;

    return json_msg;
}
Json::Value geometry_msgsToJson(const geometry_msgs::msg::Point32 &msg)
{
    Json::Value json_msg;
    json_msg["x"] = msg.x;
    json_msg["y"] = msg.y;
    json_msg["z"] = msg.z;

    return json_msg;
}
Json::Value geometry_msgsToJson(const geometry_msgs::msg::PointStamped &msg)
{
    Json::Value json_msg;
    json_msg["header"] = std_msgsToJson(msg.header);
    json_msg["point"] = geometry_msgsToJson(msg.point);

    return json_msg;
}

/*Polygon*/
Json::Value geometry_msgsToJson(const geometry_msgs::msg::Polygon &msg)
{
    Json::Value ptArray(Json::arrayValue);
    for (auto &pt : msg.points)
    {
        ptArray.append(geometry_msgsToJson(pt));
    }

    Json::Value json_msg;
    json_msg["points"] = ptArray;

    return json_msg;
}
Json::Value geometry_msgsToJson(const geometry_msgs::msg::PolygonStamped &msg)
{
    Json::Value json_msg;
    json_msg["header"] = std_msgsToJson(msg.header);
    json_msg["polygon"] = geometry_msgsToJson(msg.polygon);

    return json_msg;
}

/*Pose*/
Json::Value geometry_msgsToJson(const geometry_msgs::msg::Pose &msg)
{
    Json::Value json_msg;
    json_msg["position"] = geometry_msgsToJson(msg.position);
    json_msg["orientation"] = geometry_msgsToJson(msg.orientation);

    return json_msg;
}
Json::Value geometry_msgsToJson(const geometry_msgs::msg::Pose2D &msg)
{
    Json::Value json_msg;
    json_msg["x"] = msg.x;
    json_msg["y"] = msg.y;
    json_msg["theta"] = msg.theta;

    return json_msg;
}
Json::Value geometry_msgsToJson(const geometry_msgs::msg::PoseArray &msg)
{
    Json::Value posArray(Json::arrayValue);
    for (auto &pos : msg.poses)
    {
        posArray.append(geometry_msgsToJson(pos));
    }

    Json::Value json_msg;
    json_msg["header"] = std_msgsToJson(msg.header);
    json_msg["poses"] = posArray;

    return json_msg;
}
Json::Value geometry_msgsToJson(const geometry_msgs::msg::PoseStamped &msg)
{
    Json::Value json_msg;
    json_msg["header"] = std_msgsToJson(msg.header);
    json_msg["pose"] = geometry_msgsToJson(msg.pose);

    return json_msg;
}
Json::Value geometry_msgsToJson(const geometry_msgs::msg::PoseWithCovariance &msg)
{
    Json::Value covArray(Json::arrayValue);
    for (auto &cov : msg.covariance)
    {
        covArray.append(cov);
    }

    Json::Value json_msg;
    json_msg["pose"] = geometry_msgsToJson(msg.pose);
    json_msg["covariance"] = covArray;

    return json_msg;
}
Json::Value geometry_msgsToJson(const geometry_msgs::msg::PoseWithCovarianceStamped &msg)
{
    Json::Value json_msg;
    json_msg["header"] = std_msgsToJson(msg.header);
    json_msg["pose"] = geometry_msgsToJson(msg.pose);

    return json_msg;
}

/*Quaternion*/
Json::Value geometry_msgsToJson(const geometry_msgs::msg::Quaternion &msg)
{
    Json::Value json_msg;
    json_msg["x"] = msg.x;
    json_msg["y"] = msg.y;
    json_msg["z"] = msg.z;
    json_msg["w"] = msg.w;

    return json_msg;
}
Json::Value geometry_msgsToJson(const geometry_msgs::msg::QuaternionStamped &msg)
{
    Json::Value json_msg;
    json_msg["header"] = std_msgsToJson(msg.header);
    json_msg["quaternion"] = geometry_msgsToJson(msg.quaternion);

    return json_msg;
}

/*Transform*/
Json::Value geometry_msgsToJson(const geometry_msgs::msg::Transform &msg)
{
    Json::Value json_msg;
    json_msg["translation"] = geometry_msgsToJson(msg.translation);
    json_msg["rotation"] = geometry_msgsToJson(msg.rotation);

    return json_msg;
}
Json::Value geometry_msgsToJson(const geometry_msgs::msg::TransformStamped &msg)
{
    Json::Value json_msg;
    json_msg["header"] = std_msgsToJson(msg.header);
    json_msg["child_frame_id"] = msg.child_frame_id;
    json_msg["transform"] = geometry_msgsToJson(msg.transform);

    return json_msg;
}

/*Twist*/
Json::Value geometry_msgsToJson(const geometry_msgs::msg::Twist &msg)
{
    Json::Value json_msg;
    json_msg["linear"] = geometry_msgsToJson(msg.linear);
    json_msg["angular"] = geometry_msgsToJson(msg.angular);

    return json_msg;
}
Json::Value geometry_msgsToJson(const geometry_msgs::msg::TwistStamped &msg)
{
    Json::Value json_msg;
    json_msg["header"] = std_msgsToJson(msg.header);
    json_msg["twist"] = geometry_msgsToJson(msg.twist);

    return json_msg;
}
Json::Value geometry_msgsToJson(const geometry_msgs::msg::TwistWithCovariance &msg)
{
    Json::Value covArray(Json::arrayValue);
    for (auto &cov : msg.covariance)
    {
        covArray.append(cov);
    }

    Json::Value json_msg;
    json_msg["twist"] = geometry_msgsToJson(msg.twist);
    json_msg["covariance"] = covArray;

    return json_msg;
}
Json::Value geometry_msgsToJson(const geometry_msgs::msg::TwistWithCovarianceStamped &msg)
{
    Json::Value json_msg;
    json_msg["header"] = std_msgsToJson(msg.header);
    json_msg["twist"] = geometry_msgsToJson(msg.twist);

    return json_msg;
}

/*Vector3*/
Json::Value geometry_msgsToJson(const geometry_msgs::msg::Vector3 &msg)
{
    Json::Value json_msg;
    json_msg["x"] = msg.x;
    json_msg["y"] = msg.y;
    json_msg["z"] = msg.z;

    return json_msg;
}
Json::Value geometry_msgsToJson(const geometry_msgs::msg::Vector3Stamped &msg)
{
    Json::Value json_msg;
    json_msg["header"] = std_msgsToJson(msg.header);
    json_msg["vector"] = geometry_msgsToJson(msg.vector);

    return json_msg;
}

/*Wrench*/
Json::Value geometry_msgsToJson(const geometry_msgs::msg::Wrench &msg)
{
    Json::Value json_msg;
    json_msg["force"] = geometry_msgsToJson(msg.force);
    json_msg["torque"] = geometry_msgsToJson(msg.torque);

    return json_msg;
}
Json::Value geometry_msgsToJson(const geometry_msgs::msg::WrenchStamped &msg)
{
    Json::Value json_msg;
    json_msg["header"] = std_msgsToJson(msg.header);
    json_msg["wrench"] = geometry_msgsToJson(msg.wrench);

    return json_msg;
}

Json::Value sensor_msgsToJson(const sensor_msgs::msg::JointState &msg)
{
    Json::Value nameArray(Json::arrayValue);
    Json::Value posArray(Json::arrayValue);
    Json::Value velArray(Json::arrayValue);
    Json::Value effoArray(Json::arrayValue);

    for (auto &mem : msg.name)
    {
        nameArray.append(mem);
    }
    for (auto &mem : msg.position)
    {
        posArray.append(mem);
    }
    for (auto &mem : msg.velocity)
    {
        velArray.append(mem);
    }
    for (auto &mem : msg.effort)
    {
        effoArray.append(mem);
    }

    Json::Value json_msg;
    json_msg["header"] = std_msgsToJson(msg.header);
    json_msg["name"] = nameArray;
    json_msg["position"] = posArray;
    json_msg["velocity"] = velArray;
    json_msg["effort"] = effoArray;

    return json_msg;
}

Json::Value sensor_msgsToJson(const sensor_msgs::msg::ChannelFloat32 &msg)
{
    Json::Value valuesArray(Json::arrayValue);
    for (auto &mem : msg.values)
    {
        valuesArray.append(mem);
    }

    Json::Value json_msg;
    json_msg["name"] = msg.name;
    json_msg["values"] = valuesArray;

    return json_msg;
}

Json::Value sensor_msgsToJson(const sensor_msgs::msg::PointCloud &msg)
{
    Json::Value pointsArray(Json::arrayValue);
    Json::Value channelsArray(Json::arrayValue);
    for (auto &mem : msg.points)
    {
        pointsArray.append(geometry_msgsToJson(mem));
    }
    for (auto &mem : msg.channels)
    {
        channelsArray.append(sensor_msgsToJson(mem));
    }

    Json::Value json_msg;
    json_msg["header"] = std_msgsToJson(msg.header);
    json_msg["points"] = pointsArray;
    json_msg["channels"] = channelsArray;

    return json_msg;
}

Json::Value sensor_msgsToJson(const sensor_msgs::msg::LaserScan &msg)
{
    Json::Value rangesArray(Json::arrayValue);
    Json::Value intensitiesArray(Json::arrayValue);
    for (auto &mem : msg.ranges)
    {
        rangesArray.append(mem);
    }
    for (auto &mem : msg.intensities)
    {
        intensitiesArray.append(mem);
    }

    Json::Value json_msg;
    json_msg["header"] = std_msgsToJson(msg.header);
    json_msg["angle_min"] = msg.angle_min;
    json_msg["angle_max"] = msg.angle_max;
    json_msg["angle_increment"] = msg.angle_increment;
    json_msg["time_increment"] = msg.time_increment;
    json_msg["scan_time"] = msg.scan_time;
    json_msg["range_min"] = msg.range_min;
    json_msg["range_max"] = msg.range_max;
    json_msg["ranges"] = rangesArray;
    json_msg["intensities"] = intensitiesArray;

    return json_msg;
}